#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <sys/types.h>
#include <unistd.h>
#include <errno.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <pthread.h>
#include <signal.h>
#include <iconv.h>
#include <time.h>
#include <sys/time.h>
#include "transform.h"
#include "scanner.h"

#define CR              "\r"            // end flag 
#define CRLF            "\r\n"          // return   
#define CRLF_OK_CRLF    "\r\nOK\r\n"    // OK
#define CRLF_OK         "\r\nOK"        // OK
#define CRLF_ERROR      "\r\nERROR"     // ERROR
#define CRLF_ERROR_CRLF "\r\nERROR\r\n" // ERROR
#define CRLF_CMS_ERR_CRLF "+CMS ERROR:"
#define CTRL_Z          "\032"          //
#define AT_COMMAND_INTERVAL     100000 
#define CONFIG_COMMAND_TIMEOUT  5 
typedef unsigned char  byte;

static byte at_cr_lf[2] = {0x0D,0x0A};                          /* <CR><LF> */
static byte at_ok_rsp[6] = {0x0D,0x0A,0x4F,0x4B,0x0D,0x0A};     /* <CR><LF>OK<CR><LF> */

bool parse_AT_CMGRMI(const void *buf, int32_t *len, void *result);
bool parse_AT_CNBP(const void *buf, int32_t *len, void *result);
void encode_AT_CNBP(char *buf, int len, CNBP_INFO* cnbp_info);

/* byte_cmp: buf_len must >= key_len */
bool byte_cmp(const byte *buf, const byte *key, int32_t key_len)
{
    int32_t i = 0;
    for( ; i < key_len; ++i )
    {
        if( *(buf + i) != *(key + i) )
            return false;
    }
    return true;
}

bool find_str(const byte *buf, int32_t len, const byte *key, int32_t key_len, int32_t *pos)
{
    int32_t i = 0;

    for( ; i <= len - key_len; ++i )
    {
        if( byte_cmp(buf + i, key, key_len) )
        {
            *pos = i;
            return true;
        }
    }
    
    *pos = -1;   /* can't use pos value */
    return false;
}

ssize_t readmsg(int32_t fd, char * buffer, int32_t length)
{
    ssize_t len = 0;
	
	do{
		len = read(fd, buffer, length);
	}while((len == -1) && (errno == EINTR));

	if(len < 0)
	{
	    SCANNERLogError("read msg error");
	}

	return len;
}

ssize_t writemsg(int32_t fd, char * cmd, int32_t cmdLen)
{
    ssize_t len = 0;

	do
	{
		len = write(fd, cmd, cmdLen);
		if (len > 0)
		{
        	SCANNERLogInfo("[MODEM]: %s", cmd);
			usleep(AT_COMMAND_INTERVAL);
		}
	}while((len == -1) && (errno == EINTR));

	if(len < 0)
	{
	    SCANNERLogError("write command %s error", cmd);
	}

    return len;
}

int32_t ReadATResponse(int32_t Fd, char * Buffer, int32_t BufferLen, int32_t * pDataLen)
{
    int32_t len;
    if ( Fd <= 0 )
    {
        SCANNERLogError("set fd(%d) attribute failed", Fd);
        return -1;
    }

    len = readmsg(Fd, Buffer + (*pDataLen), BufferLen - (*pDataLen));	
    if (len > 0)
    {
       (*pDataLen) += len;
    }

    return len;
}

int32_t SendATCommandX(int32_t fd, char * command, int32_t commandLen, char * buffer, int32_t bufferLen, const char * flags[],
	                   int32_t flagCount, int32_t * dataLen, int32_t timeOut)
{
	uint32_t i = 0;
	int32_t j = 0;
	int32_t len = 0;
	int32_t ret = -1;
	uint32_t times = 0;
	int32_t maxFds = 0;
	struct timeval tv;
	fd_set readFds;

    if ( fd <= 0 )
    {
	     SCANNERLogWarning("SendATCommandX failed fd %d", fd);
         return -1;
    }

	*dataLen = 0;

    //serial read timeout is 200ms
	times = timeOut * (1000000 / 200000);

	//clear read buffer
	len = readmsg(fd, buffer, bufferLen);
	if (len > 0)
	{
       	SCANNERLogInfo("[MODEM]: %s", buffer);
		usleep(AT_COMMAND_INTERVAL);
	}
		
	memset(buffer, 0, bufferLen);
	
	SCANNERLogDebug("send at command %s", command);
	maxFds = fd + 1;

	//send AT command
	writemsg(fd, command, commandLen);
	
	//read the response in the limited time
	for (i = 0; i < times; i++)
	{
		FD_ZERO(&readFds);
		FD_SET(fd, &readFds);
		tv.tv_sec = 0; 
		tv.tv_usec = 200000;	

		ret = select(maxFds, &readFds, NULL, NULL, &tv);
		if (ret == -1)
		{
			continue;
		}
		
		if (FD_ISSET(fd, &readFds))
		{
			ReadATResponse( fd, buffer, bufferLen, dataLen);

			SCANNERLogInfo("[MODEM]: %s datalen %d", buffer, *dataLen);
			for (j = 0; j < flagCount; j++)
			{
				//find the flag, and break the for 
				if (strstr(buffer, flags[j]))
				{
					ReadATResponse(fd, buffer, bufferLen, dataLen);
					SCANNERLogInfo("[MODEM]: %s", buffer);
					return 0;
				}
			}
		}
	}

	SCANNERLogError("[MODEM] <%s>response time out %d<%d>", command, timeOut, times);
	return -1;
}

void encode_AT_CNBP(char *buf, int len, CNBP_INFO* cnbp_info)
{
    sprintf(buf, "AT+CNBP=0x%016llx,0x%016llx,0x%016llx\r", cnbp_info->mode, cnbp_info->lte_mode, cnbp_info->tds_mode);
}

static char option[4096] = {0};
static char tmpbuf[256] = {0};
static char buff[8192] = {0};

bool parse_AT_CNBP(const void *buf, int32_t *len, void *result)
{
    CNBP_INFO *info_ptr = (CNBP_INFO*)result;
    int32_t pos = 0, len_parsed = 0;
    byte *ptr = (byte *)buf;
    SCANNERLogDebug( "buf %s \n", buf);

    if (strncmp((char*)buf, "AT+", 3) == 0){
        find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos);
        ptr += pos + 2;
        len_parsed += pos + 2;
    }else{
        /* skip <CR><LF> */
        ptr += 2;
        len_parsed += 2;
    }

    /* find option end */
    if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
    {
        SCANNERLogError("wrong AT CMGRMI response, no CR-LF found");

        return false;
    }

    /* pos = option_end + 1 */
    memcpy(option, ptr, pos);
    option[pos] = '\0';

    ptr += pos;
    len_parsed += pos;

    //printf("%s\n",option);
    sscanf(option, "+CNBP: %llx,%llx,%llx", &info_ptr->mode, &info_ptr->lte_mode, &info_ptr->tds_mode);

    SCANNERLogDebug("CNBP %016llx,%016llx,%016llx", info_ptr->mode, info_ptr->lte_mode, info_ptr->tds_mode);
    /* skip <CR><LR> */
    ptr += 4;
    len_parsed += 4;

    /* check OK rsp */
    if( (*len - len_parsed >= 6) && byte_cmp(ptr, at_ok_rsp, 6) )
    {
        ptr += 6;   /* skip OK */
        len_parsed += 6;
    }
    else
    {
        SCANNERLogError("wrong AT CMGRMI response, no OK found");

        return false;
    }

    *len = len_parsed;
    return true;
}

bool parse_AT_CMGRMI(const void *buf, int32_t *len, void *result)
{
    CMGRMI_INFO *cmgrmi_info_ptr = (CMGRMI_INFO*)result;
    int32_t pos = 0, len_parsed = 0;
    byte *ptr = (byte *)buf;
    SCANNERLogDebug( "buf %s \n", buf);
    if (strncmp((char*)buf, "AT+", 3) == 0){
        find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos);
        ptr += pos + 2;
        len_parsed += pos + 2;
    }else{
        /* skip <CR><LF> */
        ptr += 2;
        len_parsed += 2;
    }

    /* find option end */
    if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
    {
        SCANNERLogError("wrong AT CMGRMI response, no CR-LF found");

        return false;
    }

    /* pos = option_end + 1 */
    memcpy(option, ptr, pos);
    option[pos] = '\0';

    ptr += pos;
    len_parsed += pos;

    CMGRMI_MAIN_INFO *main_info = &(cmgrmi_info_ptr->main_info);
    //printf("%s\n",option);
    int args = sscanf(option, "+CMGRMI: Main_Info,%d,%d,%d,%d,%d,%d,%d,%d", &main_info->mode, &main_info->service_available, &main_info->valid, &main_info->idle, &main_info->ra_rnti, &main_info->c_rnti, &main_info->cqi_wb, &main_info->enb_num_tx_antenna);

    if (args < 8)
        return false;

    SCANNERLogInfo( "Main_Info %d,%d,%d,%d,%d,%d,%d,%d", main_info->mode, main_info->service_available, main_info->valid, main_info->idle, main_info->ra_rnti, main_info->c_rnti, main_info->cqi_wb, main_info->enb_num_tx_antenna);

    /* skip <CR><LF> */
    ptr += 4;
    len_parsed += 4;

    /* find option end */
    if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
    {
        SCANNERLogError("wrong AT CMGRMI response, no CR-LF found");

        return false;
    }

    /* pos = option_end + 1 */
    memcpy(option, ptr, pos);
    option[pos] = '\0';

    ptr += pos;
    len_parsed += pos;

    CMGRMI_SERVING_CELL *serving_cell = &(cmgrmi_info_ptr->serving_cell);
    //printf("%s\n",option);
    int mcc,mnc;
    mcc = 0; mnc = 0;
    args = sscanf(option, "+CMGRMI: Serving_Cell,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d", &serving_cell->earfcn, &mcc, &mnc, &serving_cell->tac, &serving_cell->num_mnc_digits, &serving_cell->serving_cell_id, &serving_cell->freq_band_ind, &serving_cell->dl_bandwidth, &serving_cell->ul_bandwidth, &serving_cell->serv_rssnr, &serving_cell->cell_pci, &serving_cell->cell_rsrq, &serving_cell->cell_rsrp, &serving_cell->cell_rssi, &serving_cell->cell_idle_srxlev);
    sprintf(serving_cell->mcc, "%d", mcc);
    sprintf(serving_cell->mnc, "%02d", mnc);

    SCANNERLogInfo("Serving_Cell %d,%s,%s,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d", serving_cell->earfcn, serving_cell->mcc, serving_cell->mnc, serving_cell->tac, serving_cell->num_mnc_digits,serving_cell->serving_cell_id, serving_cell->freq_band_ind, serving_cell->dl_bandwidth, serving_cell->ul_bandwidth, serving_cell->serv_rssnr, serving_cell->cell_pci, serving_cell->cell_rsrq, serving_cell->cell_rsrp, serving_cell->cell_rssi, serving_cell->cell_idle_srxlev);
 
    /* skip <CR><LR> */
    ptr += 4;
    len_parsed += 4;

    /* find option end */
    if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
    {
        SCANNERLogError("wrong AT CMGRMI response, no CR-LF found");

        return false;
    }

    /* pos = option_end + 1 */
    memcpy(option, ptr, pos);
    option[pos] = '\0';

    ptr += pos;
    len_parsed += pos;

    CMGRMI_LTE_INTRA *lte_intra = &(cmgrmi_info_ptr->lte_intra);
    //printf("%s\n",option);
    sscanf(option, "+CMGRMI: LTE_Intra,%d,%d,%d,%d", &lte_intra->sib3_received, &lte_intra->earfcn, &lte_intra->serving_cell_id, &lte_intra->num_lte_cells);

    SCANNERLogInfo("LTE_Intra %d,%d,%d,%d", lte_intra->sib3_received, lte_intra->earfcn, lte_intra->serving_cell_id, lte_intra->num_lte_cells);

    if (lte_intra->num_lte_cells < 0 || lte_intra->num_lte_cells > 8)
        return false;
 
    for(uint32_t i= 0; i < lte_intra->num_lte_cells; i++){
        int c;
     
        /* skip <CR><LR> */
        ptr += 4;
        len_parsed += 4;

        /* find option end */
        if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
        {
            SCANNERLogError("wrong AT CMGRMI response, no CR-LF found");

            return false;
        }

        /* pos = option_end + 1 */
        memcpy(option, ptr, pos);
        option[pos] = '\0';

        ptr += pos;
        len_parsed += pos;

        CMGRMI_LTE_INTRA_CELL *lte_intra_cell = &cmgrmi_info_ptr->lte_intra_cell[i];
        sscanf(option, "+CMGRMI: LTE_Intra_Cell%d,%d,%d,%d,%d,%d", &c, &lte_intra_cell->cell_pci, &lte_intra_cell->cell_rsrq, &lte_intra_cell->cell_rsrp, &lte_intra_cell->cell_rssi, &lte_intra_cell->cell_idle_srxlev);

        SCANNERLogInfo("LTE_Intra_Cell%d %d,%d,%d,%d,%d", c, lte_intra_cell->cell_pci, lte_intra_cell->cell_rsrq, lte_intra_cell->cell_rsrp, lte_intra_cell->cell_rssi, lte_intra_cell->cell_idle_srxlev);

    };

 
    /* skip <CR><LR> */
    ptr += 4;
    len_parsed += 4;

    /* find option end */
    if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
    {
        SCANNERLogError("wrong AT CMGRMI response, no CR-LF found");

        return false;
    }

    /* pos = option_end + 1 */
    memcpy(option, ptr, pos);
    option[pos] = '\0';

    ptr += pos;
    len_parsed += pos;

    CMGRMI_LTE_INTER *lte_inter = &(cmgrmi_info_ptr->lte_inter);
    //printf("%s\n",option);
    sscanf(option, "+CMGRMI: LTE_Inter,%d,Freq1,%d,%d,%d,%d,%d,Freq2,%d,%d,%d,%d,%d", &lte_inter->num_freqs, &lte_inter->Freq1.earfcn, &lte_inter->Freq1.num_lte_cells, &lte_inter->Freq1.idle_threshX_low, &lte_inter->Freq1.idle_threshX_high, &lte_inter->Freq1.idle_cell_resel_priority, &lte_inter->Freq2.earfcn, &lte_inter->Freq2.num_lte_cells, &lte_inter->Freq2.idle_threshX_low, &lte_inter->Freq2.idle_threshX_high, &lte_inter->Freq2.idle_cell_resel_priority);

    SCANNERLogInfo("LTE_Inter %d,Freq1,%d,%d,%d,%d,%d,Freq2,%d,%d,%d,%d,%d", lte_inter->num_freqs, lte_inter->Freq1.earfcn, lte_inter->Freq1.num_lte_cells, lte_inter->Freq1.idle_threshX_low, lte_inter->Freq1.idle_threshX_high, lte_inter->Freq1.idle_cell_resel_priority, lte_inter->Freq2.earfcn, lte_inter->Freq2.num_lte_cells, lte_inter->Freq2.idle_threshX_low, lte_inter->Freq2.idle_threshX_high, lte_inter->Freq2.idle_cell_resel_priority);

    if (lte_inter->Freq1.num_lte_cells < 0 || lte_inter->Freq1.num_lte_cells > 8)
        return false;

    for(uint32_t i= 0; i < lte_inter->Freq1.num_lte_cells; i++){
        int c;
     
        /* skip <CR><LR> */
        ptr += 4;
        len_parsed += 4;

        /* find option end */
        if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
        {
            SCANNERLogError("wrong AT CMGRMI response, no CR-LF found");

            return false;
        }

        /* pos = option_end + 1 */
        memcpy(option, ptr, pos);
        option[pos] = '\0';

        ptr += pos;
        len_parsed += pos;

        CMGRMI_LTE_INTER_CELL *lte_inter_cell = &cmgrmi_info_ptr->lte_inter_freq1_cell[i];
        sscanf(option, "+CMGRMI: LTE_InterFreq1_Cell%d,%d,%d,%d,%d,%d", &c, &lte_inter_cell->cell_pci, &lte_inter_cell->cell_rsrq, &lte_inter_cell->cell_rsrp, &lte_inter_cell->cell_rssi, &lte_inter_cell->cell_idle_srxlev);

        SCANNERLogInfo("LTE_InterFreq1_Cell %d,%d,%d,%d,%d", lte_inter_cell->cell_pci, lte_inter_cell->cell_rsrq, lte_inter_cell->cell_rsrp, lte_inter_cell->cell_rssi, lte_inter_cell->cell_idle_srxlev);

    };

    if (lte_inter->Freq2.num_lte_cells < 0 || lte_inter->Freq2.num_lte_cells > 8)
        return false;

    for(uint32_t i= 0; i < lte_inter->Freq2.num_lte_cells; i++)
    {
        int c;
     
        /* skip <CR><LR> */
        ptr += 4;
        len_parsed += 4;

        /* find option end */
        if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
        {
            SCANNERLogError("wrong AT CMGRMI response, no CR-LF found");

            return false;
        }

        /* pos = option_end + 1 */
        memcpy(option, ptr, pos);
        option[pos] = '\0';

        ptr += pos;
        len_parsed += pos;

        CMGRMI_LTE_INTER_CELL *lte_inter_cell = &cmgrmi_info_ptr->lte_inter_freq2_cell[i];
        sscanf(option, "+CMGRMI: LTE_InterFreq2_Cell%d,%d,%d,%d,%d,%d", &c, &lte_inter_cell->cell_pci, &lte_inter_cell->cell_rsrq, &lte_inter_cell->cell_rsrp, &lte_inter_cell->cell_rssi, &lte_inter_cell->cell_idle_srxlev);

        SCANNERLogInfo("LTE_InterFreq2_Cell %d,%d,%d,%d,%d", lte_inter_cell->cell_pci, lte_inter_cell->cell_rsrq, lte_inter_cell->cell_rsrp, lte_inter_cell->cell_rssi, lte_inter_cell->cell_idle_srxlev);
    };

    /* skip <CR><LR> */
    ptr += 4;
    len_parsed += 4;

    /* check OK rsp */
    if( (*len - len_parsed >= 6) && byte_cmp(ptr, at_ok_rsp, 6) )
    {
        ptr += 6;   /* skip OK */
        len_parsed += 6;
    }
    else
    {
        SCANNERLogError("wrong AT CMGRMI response, no OK found");

        return false;
    }

    *len = len_parsed;
    return true;
}

uint64_t DEFAULT_BANDS = 0x000007FF03DF3FFFull;
uint64_t DEFAULT_MODE = 0x0002000000400183ull;
uint64_t DEFAULT_TDS_MODE = 0x000000000000003Full;

static CNBP_INFO cnbp_info;

bool query_lte_cell_info(int32_t sms_serial_fd,CMGRMI_INFO *cmgrmi_info, uint64_t band)
{
    const char *FlagListOk[2] = { CRLF_OK_CRLF, CRLF_CMS_ERR_CRLF};
    int32_t rspLen = 0;
    int32_t dataLen = 0;

    /* set allow fully function */
    char setfc[15] = { "AT+CFUN=1\r" } ;
    rspLen = SendATCommandX(sms_serial_fd, setfc, strlen(setfc), buff, 8192, FlagListOk,
	                        2,  &dataLen, CONFIG_COMMAND_TIMEOUT);
    if ( rspLen == 0 &&  dataLen >= 0)
    {
        //AT_SYSINFO at_sysinfo;
        //parse_AT_SYSINFO(buff, &dataLen, &at_sysinfo);
        SCANNERLogError("set %s", buff);
    }

    /* set allow limited service */
    char cnlsa[15] = { "AT+CNLSA=1\r" } ;
    rspLen = SendATCommandX(sms_serial_fd, cnlsa, strlen(cnlsa), buff, 8192, FlagListOk,
	                        2,  &dataLen, CONFIG_COMMAND_TIMEOUT);
    if ( rspLen == 0 &&  dataLen >= 0)
    {
        //AT_SYSINFO at_sysinfo;
        //parse_AT_SYSINFO(buff, &dataLen, &at_sysinfo);
        SCANNERLogDebug("CNMP %s \n", buff);
    }

    /* set lte only */
    char SYSINFO[15] = { "AT+CNMP=38\r" } ;
    rspLen = SendATCommandX(sms_serial_fd, SYSINFO, strlen(SYSINFO), buff, 8192, FlagListOk,
	                        2,  &dataLen, CONFIG_COMMAND_TIMEOUT);
    if ( rspLen == 0 &&  dataLen >= 0)
    {
        //AT_SYSINFO at_sysinfo;
        //parse_AT_SYSINFO(buff, &dataLen, &at_sysinfo);
        SCANNERLogDebug("CNMP %s", buff);
    }
    memset(buff, 0, 8192);
    dataLen = 0;   
    
    /* parse cnbp */
    char cnbpinfo[15] = { "AT+CNBP?\r" } ;
    rspLen = SendATCommandX(sms_serial_fd, cnbpinfo, strlen(cnbpinfo), buff, 8192, FlagListOk,
	                        2,  &dataLen, CONFIG_COMMAND_TIMEOUT);
    if ( rspLen == 0 &&  dataLen >= 0)
    {
        parse_AT_CNBP(buff, &dataLen, &cnbp_info);
        SCANNERLogDebug("CNMP %s", buff);
    }
    memset(buff, 0, 8192);
    dataLen = 0;   

    /* set cnbp */
    char cnbpinfoset[256] = { 0} ;
    cnbp_info.lte_mode = band;
    cnbp_info.mode = DEFAULT_MODE;
    cnbp_info.tds_mode = DEFAULT_TDS_MODE;

    encode_AT_CNBP(cnbpinfoset, sizeof(cnbpinfoset),&cnbp_info);
    rspLen = SendATCommandX(sms_serial_fd, cnbpinfoset, strlen(cnbpinfoset), buff, 8192, FlagListOk,
	                        2,  &dataLen, 10);
    if ( rspLen == 0 &&  dataLen >= 0)
    {
        SCANNERLogDebug("set CNBP %s", buff);
    }
    memset(buff, 0, 8192);
    dataLen = 0;   

    sleep(10);

    /* get lte frequency */
    char CMGRMIINFO[15] = { "AT+CMGRMI=4\r" } ;
    rspLen = SendATCommandX(sms_serial_fd, CMGRMIINFO, strlen(CMGRMIINFO), buff, 8192, FlagListOk,
	                        2,  &dataLen, 30);
    if ( rspLen == 0 &&  dataLen >= 0)
    {
        parse_AT_CMGRMI(buff, &dataLen, cmgrmi_info);
        SCANNERLogDebug("CMGRMI %s", buff);
    }
    memset(buff, 0, 8192);
    dataLen = 0;

    return true;
}

bool parse_AT_CRUS(const void *buf, int32_t *len, void *result)
{
    char* tmpptr;
    CRUS_INFO *crus_info_ptr = (CRUS_INFO*)result;
    int32_t pos = 0, len_parsed = 0;
    byte *ptr = (byte *)buf;
    SCANNERLogDebug( "buf %s", buf);

    if (strncmp((char*)buf, "AT+", 3) == 0){
        find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos);
        ptr += pos + 2;
        len_parsed += pos + 2;
    }else{
        /* skip <CR><LF> */
        ptr += 2;
        len_parsed += 2;
    }

    /* find option end */
    if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
    {
        SCANNERLogError("wrong AT CRUS response, no CR-LF found");

        return false;
    }

    /* pos = option_end + 1 */
    memcpy(option, ptr, pos);
    option[pos] = '\0';

    ptr += pos;
    len_parsed += pos;

    int args = sscanf(option, "+CRUS: Active SET,%d", &crus_info_ptr->active_set_cells_num);
    sprintf(tmpbuf, "+CRUS: Active SET,%d", crus_info_ptr->active_set_cells_num);
    SCANNERLogDebug( tmpbuf);
    tmpptr = option + strlen(tmpbuf);
    for (int i = 0; i < crus_info_ptr->active_set_cells_num; i++)
    {
        ACTIVE_SET *active_set = &crus_info_ptr->active_set[i];
        sscanf(tmpptr, ",%d,%d,%d,%d,%f,%f,%d,%d,%d,%d", &active_set->cell_psc, &active_set->cell_freq, &active_set->cell_ssc, &active_set->cell_sstd, &active_set->cell_totecio, &active_set->cell_ecio, &active_set->cell_rscp, &active_set->umts_sets_cell_tpc, &active_set->umts_sets_cell_seccpichovsf, &active_set->cell_winsize);
        sprintf(tmpbuf, ",%d,%d,%d,%d,%.1f,%.1f,%d,%d,%d,%d", active_set->cell_psc, active_set->cell_freq, active_set->cell_ssc, active_set->cell_sstd, active_set->cell_totecio, active_set->cell_ecio, active_set->cell_rscp, active_set->umts_sets_cell_tpc, active_set->umts_sets_cell_seccpichovsf, active_set->cell_winsize);
        SCANNERLogDebug( tmpbuf);
        //printf(tmpbuf);printf("\n");
        tmpptr += strlen(tmpbuf);
    }
     
    /* skip <CR><LR> */
    ptr += 4;
    len_parsed += 4;

    /* find option end */
    if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
    {
        SCANNERLogError("wrong AT CRUS response, no CR-LF found");

        return false;
    }

    /* pos = option_end + 1 */
    memcpy(option, ptr, pos);
    option[pos] = '\0';

    ptr += pos;
    len_parsed += pos;

    args = sscanf(option, "+CRUS: Sync Neighbor SET,%d", &crus_info_ptr->syncset_cells_nums);
    sprintf(tmpbuf, "+CRUS: Sync Neighbor SET,%d", crus_info_ptr->syncset_cells_nums);
    SCANNERLogDebug( tmpbuf);
    tmpptr = option + strlen(tmpbuf);
    for (int i = 0; i < crus_info_ptr->syncset_cells_nums; i++)
    {
        NEIGHBOR_SET *active_set = &crus_info_ptr->sync_neighbor_set[i];
        sscanf(tmpptr, ",%d,%d,%d,%d,%f,%f,%d,%d", &active_set->cell_psc, &active_set->cell_freq, &active_set->cell_ssc, &active_set->cell_sstd, &active_set->cell_totecio, &active_set->cell_ecio, &active_set->cell_rscp, &active_set->cell_winsize);
        //printf("nset:%d\n", args);
        sprintf(tmpbuf, ",%d,%d,%d,%d,%.1f,%.1f,%d,%d", active_set->cell_psc, active_set->cell_freq, active_set->cell_ssc, active_set->cell_sstd, active_set->cell_totecio, active_set->cell_ecio, active_set->cell_rscp, active_set->cell_winsize);
        SCANNERLogDebug( tmpbuf);
        //printf(tmpbuf);printf("\n");
        tmpptr += strlen(tmpbuf);
    }
     
    /* skip <CR><LR> */
    ptr += 4;
    len_parsed += 4;

    /* find option end */
    if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
    {
        SCANNERLogError( "wrong AT CRUS response, no CR-LF found");

        return false;
    }

    /* pos = option_end + 1 */
    memcpy(option, ptr, pos);
    option[pos] = '\0';

    ptr += pos;
    len_parsed += pos;

    args = sscanf(option, "+CRUS: Async Neighbor SET,%d", &crus_info_ptr->asyncset_cells_nums);
    sprintf(tmpbuf, "+CRUS: Async Neighbor SET,%d", crus_info_ptr->asyncset_cells_nums);
    SCANNERLogDebug( tmpbuf);
    tmpptr = option + strlen(tmpbuf);
    for (int i = 0; i < crus_info_ptr->asyncset_cells_nums; i++)
    {
        NEIGHBOR_SET *active_set = &crus_info_ptr->async_neighbor_set[i];
        sscanf(tmpptr, ",%d,%d,%d,%d,%f,%f,%d,%d", &active_set->cell_psc, &active_set->cell_freq, &active_set->cell_ssc, &active_set->cell_sstd, &active_set->cell_totecio, &active_set->cell_ecio, &active_set->cell_rscp, &active_set->cell_winsize);
        //printf("nset:%d\n", args);
        sprintf(tmpbuf, ",%d,%d,%d,%d,%.1f,%.1f,%d,%d", active_set->cell_psc, active_set->cell_freq, active_set->cell_ssc, active_set->cell_sstd, active_set->cell_totecio, active_set->cell_ecio, active_set->cell_rscp, active_set->cell_winsize);
        SCANNERLogDebug( tmpbuf);
        //printf(tmpbuf);printf("\n");
        tmpptr += strlen(tmpbuf);
    }

    return true;
}


bool query_wcdma_cell_info(int32_t sms_serial_fd,CRUS_INFO *crus_info)
{
    const char *FlagListOk[2] = { CRLF_OK_CRLF, CRLF_CMS_ERR_CRLF};
    int32_t rspLen = 0;
    int32_t dataLen = 0;

    /* set allow limited service */
    char cnlsa[15] = { "AT+CNLSA=1\r" } ;
    rspLen = SendATCommandX(sms_serial_fd, cnlsa, strlen(cnlsa), buff, 8192, FlagListOk,
	                        2,  &dataLen, CONFIG_COMMAND_TIMEOUT);
    if ( rspLen == 0 &&  dataLen >= 0)
    {
        //AT_SYSINFO at_sysinfo;
        //parse_AT_SYSINFO(buff, &dataLen, &at_sysinfo);
        SCANNERLogDebug("CNMP %s", buff);
    }

    /* set lte only */
    char SYSINFO[15] = { "AT+CNMP=14\r" } ;
    rspLen = SendATCommandX(sms_serial_fd, SYSINFO, strlen(SYSINFO), buff, 8192, FlagListOk,
	                        2,  &dataLen, CONFIG_COMMAND_TIMEOUT);
    if ( rspLen == 0 &&  dataLen >= 0)
    {
        //AT_SYSINFO at_sysinfo;
        //parse_AT_SYSINFO(buff, &dataLen, &at_sysinfo);
        SCANNERLogDebug("CNMP %s", buff);
    }
    memset(buff, 0, 8192);
    dataLen = 0;   
    
    sleep(10);

    /* get lte frequency */
    char CMGRMIINFO[15] = { "AT+CRUS\r" } ;
    rspLen = SendATCommandX(sms_serial_fd, CMGRMIINFO, strlen(CMGRMIINFO), buff, 8192, FlagListOk,
	                        2,  &dataLen, 30);
    if ( rspLen == 0 &&  dataLen >= 0)
    {
        parse_AT_CRUS(buff, &dataLen, crus_info);
        SCANNERLogDebug("CRUS %s", buff);
    }
    memset(buff, 0, 8192);
    dataLen = 0;

    return true;
}

bool parse_AT_MONI(const void *buf, int32_t *len, void *result)
{
    char* tmpptr;
    MONI_INFO *moni_info_ptr = (MONI_INFO*)result;
    int32_t pos = 0, len_parsed = 0;
    byte *ptr = (byte *)buf;
    SCANNERLogDebug( "buf %s \n", buf);

    if (strncmp((char*)buf, "AT+", 3) == 0){
        find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos);
        ptr += pos + 2;
        len_parsed += pos + 2;
    }else{
        /* skip <CR><LF> */
        ptr += 2;
        len_parsed += 2;
    }

    /* find option end */
    if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
    {
        SCANNERLogError( "wrong AT MONI response, no CR-LF found");

        return false;
    }

    /* pos = option_end + 1 */
    memcpy(option, ptr, pos);
    option[pos] = '\0';

    ptr += pos;
    len_parsed += pos;

    GSM_SERVING_CELL *serving_cell = &moni_info_ptr->serving_cell;
    int mnc = 0;
    int mcc = 0;
    //+MONI: Cc: 460,Nc: 00,BSIC: 25,RXQual: 255,LAC: 10306,Id: -379785747,ARFCN: 556,PWR: -85dBm,C1: 20-C2: 130
    int args = sscanf(option, "+MONI: Cc: %d,Nc: %d,BSIC: %d,RXQual: %d,LAC: %d,Id: %d, ARFCN: %d,PWR: %ddBm,C1: %d-C2: %d", &mcc, &mnc, &serving_cell->bsic, &serving_cell->rxqual, &serving_cell->lac, &serving_cell->id, &serving_cell->arfcn, &serving_cell->pwr, &serving_cell->c1, &serving_cell->c2);
    if (args < 10){
        char netname[128] = {0};
        //printf(option);
        args = sscanf(option, "+MONI: %[^,]", netname);
        //printf("name:%s\n", netname);
        tmpptr = option + strlen("+MONI: ") + strlen(netname);
        args = sscanf(tmpptr, ",BSIC: %d,RXQual: %d,LAC: %d,Id: %d, ARFCN: %d,PWR: %ddBm,C1: %d-C2: %d", &serving_cell->bsic, &serving_cell->rxqual, &serving_cell->lac, &serving_cell->id, &serving_cell->arfcn, &serving_cell->pwr, &serving_cell->c1, &serving_cell->c2);

        if (args < 8){
            SCANNERLogError("failed to parse string:%d");
            return false;
        }else{
            if (strstr(netname, "CMCC") != NULL || strstr(netname, "Mobile") != NULL){
                mcc = 460;
                mnc = 0;
            }else if (strstr(netname, "UNICOM") != NULL){
                mcc = 460;
                mnc = 1;
            }
        }
    }
    sprintf(serving_cell->mcc, "%d", mcc);
    sprintf(serving_cell->mnc, "%02d", mnc);

    /* skip <CR><LR> */
    ptr += 4;
    len_parsed += 4;

    /* find option end */
    if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
    {
        SCANNERLogError("wrong AT MONI response, no CR-LF found");

        return false;
    }

    /* pos = option_end + 1 */
    memcpy(option, ptr, pos);
    option[pos] = '\0';

    ptr += pos;
    len_parsed += pos;

    int c = 0;
    for (int i = 0; i < 6; i++)
    {
        GSM_NEIGHBOR_CELL *neighbor_cell = &moni_info_ptr->neighbor_cell[i];
        //+MONI: Adj Cell1,[LAC: 10300,Id: 55889],ARFCN: 30,PWR: -87dBm,C1: 12-C2: 102
        int args = sscanf(option, "+MONI: Adj Cell%d,[LAC: %d,Id: %d],ARFCN: %d,PWR: %ddBm, C1: %d-C2: %d", &c, &neighbor_cell->lac, &neighbor_cell->id, &neighbor_cell->arfcn, &neighbor_cell->pwr, &neighbor_cell->c1, &neighbor_cell->c2);
        if (args < 7)
            break;
     
        /* skip <CR><LR> */
        ptr += 4;
        len_parsed += 4;

        /* find option end */
        if( !find_str(ptr, *len - len_parsed, at_cr_lf, 2, &pos) )
        {
            SCANNERLogError("wrong AT MONI response, no CR-LF found");

            return false;
        }

        /* pos = option_end + 1 */
        memcpy(option, ptr, pos);
        option[pos] = '\0';

        ptr += pos;
        len_parsed += pos;
    }

    moni_info_ptr->neighbor_cells_num = c;

    return true;
}

/*
+MONI:<netname>,BSIC:<bsic>,RxQual:<qual>,LAC:<lac>,Id:<i
d>, ARFCN:<arfcn>,PWR:<dBm>dBm,C1: <C1>-C2: <C2>
+MONI:Adj Cell<n>,[LAC:<lac>,Id:<id>],ARFCN:<arfcn>,PWR:
<dBm>dBm, C1: <C1>-C2: <C2>
AT+MONI
+MONI: Cc: 460,Nc: 00,BSIC: 25,RXQual: 255,LAC: 10306,Id: -379785747,ARFCN: 556,PWR: -85dBm,C1: 20-C2: 130

+MONI: Adj Cell1,[LAC: 10300,Id: 59584],ARFCN: 542,PWR: -89dBm,C1: 16-C2: 122

+MONI: Adj Cell2,[LAC: 10300,Id: 55889],ARFCN: 30,PWR: -85dBm,C1: 14-C2: 104


*/

bool query_gsm_cell_info(int32_t sms_serial_fd,MONI_INFO *moni_info)
{
    const char *FlagListOk[3] = { CRLF_OK_CRLF, CRLF_CMS_ERR_CRLF, CRLF_ERROR_CRLF};
    int32_t rspLen = 0;
    int32_t dataLen = 0;

    /* set allow limited service */
    char cnlsa[15] = { "AT+CNLSA=1\r" } ;
    rspLen = SendATCommandX(sms_serial_fd, cnlsa, strlen(cnlsa), buff, 8192, FlagListOk,
	                        3,  &dataLen, CONFIG_COMMAND_TIMEOUT);
    if ( rspLen == 0 &&  dataLen >= 0)
    {
        //AT_SYSINFO at_sysinfo;
        //parse_AT_SYSINFO(buff, &dataLen, &at_sysinfo);
        SCANNERLogDebug("CNMP %s", buff);
    }

    /* set lte only */
    char SYSINFO[15] = { "AT+CNMP=13\r" } ;
    rspLen = SendATCommandX(sms_serial_fd, SYSINFO, strlen(SYSINFO), buff, 8192, FlagListOk,
	                        3,  &dataLen, CONFIG_COMMAND_TIMEOUT);
    if ( rspLen == 0 &&  dataLen >= 0)
    {
        //AT_SYSINFO at_sysinfo;
        //parse_AT_SYSINFO(buff, &dataLen, &at_sysinfo);
        SCANNERLogDebug("CNMP %s", buff);
    }
    memset(buff, 0, 8192);
    dataLen = 0;   
    
    char SETMOBILE[48] = { "AT+COPN\r" } ;
    rspLen = SendATCommandX(sms_serial_fd, SETMOBILE, strlen(SETMOBILE), buff, 8192, FlagListOk,
	                        3,  &dataLen, CONFIG_COMMAND_TIMEOUT);
    if ( rspLen == 0 &&  dataLen >= 0)
    {
        //AT_SYSINFO at_sysinfo;
        //parse_AT_SYSINFO(buff, &dataLen, &at_sysinfo);
        SCANNERLogDebug("CNMP %s", buff);
    }
    memset(buff, 0, 8192);
    dataLen = 0;   
    sleep(10);

    /* get lte frequency */
    char CMGRMIINFO[15] = { "AT+MONI\r" } ;
    rspLen = SendATCommandX(sms_serial_fd, CMGRMIINFO, strlen(CMGRMIINFO), buff, 8192, FlagListOk,
	                        3,  &dataLen, 30);
    if ( rspLen == 0 &&  dataLen >= 0)
    {
        parse_AT_MONI(buff, &dataLen, moni_info);
        SCANNERLogDebug("MONI %s", buff);
    }
    memset(buff, 0, 8192);
    dataLen = 0;

    return true;
}

